/**
 * @file global_planner.hpp
 * @brief 全局路径规划器的头文件
 * 
 * 该文件定义了全局路径规划器类，用于在点云或栅格地图上规划从起点到终点的最优路径。
 * 该规划器集成了多种规划算法，并可以根据地图类型自动选择合适的算法。
 */

#ifndef GLOBAL_PLANNER_HPP
#define GLOBAL_PLANNER_HPP

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <mutex>
#include <memory>

#include "auto_navigation/planning/planner_manager.hpp"
#include "auto_navigation/planning/astar_planner.hpp"
#include "auto_navigation/planning/pointcloud_planner.hpp"

namespace auto_navigation {
namespace planning {

/**
 * @class GlobalPlanner
 * @brief 全局路径规划器类
 * 
 * 该类实现了全局路径规划功能，支持点云地图和栅格地图，集成了多种规划算法。
 */
class GlobalPlanner {
public:
    /**
     * @brief 构造函数
     */
    GlobalPlanner();
    
    /**
     * @brief 析构函数
     */
    ~GlobalPlanner();
    
    /**
     * @brief 初始化规划器
     * @param nh 节点句柄
     * @return 是否成功初始化
     */
    bool initialize(ros::NodeHandle& nh);
    
    /**
     * @brief 设置栅格地图
     * @param map 栅格地图
     */
    void setGridMap(const nav_msgs::OccupancyGrid::ConstPtr& map);
    
    /**
     * @brief 设置点云地图
     * @param cloud 点云地图
     */
    void setPointCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud);
    
    /**
     * @brief 规划路径
     * @param start 起始位姿
     * @param goal 目标位姿
     * @param path 输出的路径
     * @return 是否成功规划
     */
    bool planPath(const geometry_msgs::PoseStamped& start,
                 const geometry_msgs::PoseStamped& goal,
                 nav_msgs::Path& path);
    
    /**
     * @brief 获取当前位姿
     * @param pose 输出的当前位姿
     * @return 是否成功获取
     */
    bool getCurrentPose(geometry_msgs::PoseStamped& pose);
    
    // 检查目标点的可行性，如果不可行，尝试寻找附近的可行点
    bool checkGoalValidity(const geometry_msgs::PoseStamped& goal_pose, 
                          geometry_msgs::PoseStamped& valid_goal_pose);
    
private:
    /**
     * @brief 栅格地图回调函数
     * @param msg 栅格地图消息
     */
    void gridMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
    
    /**
     * @brief 点云地图回调函数
     * @param msg 点云地图消息
     */
    void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
    
    /**
     * @brief 目标点回调函数
     * @param msg 目标位姿消息
     */
    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    
    ros::NodeHandle nh_;                    ///< ROS节点句柄
    ros::NodeHandle private_nh_;            ///< 私有ROS节点句柄
    
    ros::Subscriber grid_map_sub_;          ///< 栅格地图订阅器
    ros::Subscriber pointcloud_sub_;        ///< 点云地图订阅器
    ros::Subscriber goal_sub_;              ///< 目标点订阅器
    
    ros::Publisher path_pub_;               ///< 路径发布器
    ros::Publisher vis_path_pub_;           ///< 可视化路径发布器
    ros::Publisher valid_goal_pub_;          ///< 用于发布有效目标点
    
    PlannerManager planner_manager_;        ///< 规划器管理器
    std::mutex map_mutex_;                  ///< 地图互斥锁
    
    bool grid_map_received_;                ///< 是否接收到栅格地图
    bool pointcloud_received_;              ///< 是否接收到点云地图
    
    tf2_ros::Buffer tf_buffer_;             ///< TF缓冲
    tf2_ros::TransformListener tf_listener_; ///< TF监听器
    
    std::string map_frame_id_;              ///< 地图坐标系ID
    std::string base_frame_id_;             ///< 基座坐标系ID
    
    double plan_frequency_;                 ///< 规划频率
    ros::Timer planning_timer_;             ///< 规划定时器
    
    geometry_msgs::PoseStamped current_goal_; ///< 当前目标位姿
    bool has_goal_;                         ///< 是否有目标点
    
    /**
     * @brief 规划定时器回调函数
     * @param event 定时器事件
     */
    void planningTimerCallback(const ros::TimerEvent& event);
};

} // namespace planning
} // namespace auto_navigation

#endif // GLOBAL_PLANNER_HPP 